#include "ares_pathing/ares_path_planner.h"
#include "drive_commands/twist_path.h"

using namespace pathing;

int main(int argc, char **argv){
    //ROS setup
    ros::init(argc, argv, "path_planner");
    ros::NodeHandle nh; //handle ros comms
    
    //set up the transform listener
    tfl = boost::make_shared<tf::TransformListener>();

    //subscribe to the needed topics
    sub_odom = nh.subscribe("odom",2,odomCallback);
    sub_map = nh.subscribe("projected_map",1,mapCallback);
    sub_goal = nh.subscribe("move_base_simple/goal",1,goalCallback);
    //set up publisher
    pub_path = nh.advertise<drive_commands::twist_path>("path/cmd_vel",2);

    //Don't go into the loop that does operations on the map until the flag has
    //been set by the map callback.    
    while(!ready && ros::ok()){
        ROS_INFO("Map: %i Odom: %i Goal: %i",mapflag,odomflag,goalflag);
        ros::spinOnce();
        ready = mapflag && odomflag && goalflag;
    }
    //make the path!
    world->layPath();

    //main loop!
    while(ros::ok()){
        ROS_INFO("loop complete!");
        drivePub();               
        world->printBot();
        world->printGoal();
        ros::spinOnce();
        world->layPath();
    }//end the main loop

}

void pathing::drivePub(){
    drive_commands::twist_path msg;
    world->writeMsg(&msg);
    pub_path.publish(msg);
}
